#ifndef ROBOT_HPP
#define	ROBOT_HPP

#include "visualization.hpp"
#include "kalmanFilter.hpp"

#include <stdio.h>

class Robot {
public:

    Robot() : ok(false) {
    }

    virtual ~Robot() {
        delete kalman;
    }

    bool setRobotInfo(INFO_ROBOT info);
    INFO_ROBOT getRobotInfo();

    void setupKalmanFilter(int DM, int MM, int CM,
            float* F, float* B, float* H, float* Q, float* R,
            float* error_covariance, float* state);
    
    void setupKalmanFilter();
    
    void updateKalman() {
        if (this->ok)
            this->kalman->updateKalman(this->info);
    }
private:

    INFO_ROBOT info;
    Kalman<float> *kalman;
    bool ok;
};

#endif	/* ROBOT_HPP */

